#include"vld_imu_slam/show_pub.h"

namespace vld_imu_slam{

void ShowPub::pubMap(pcl::PointCloud<PointType>::Ptr p_map){
    sensor_msgs::PointCloud2 tempCloud;
    pcl::toROSMsg(*p_map, tempCloud);
    tempCloud.header.stamp = ros::Time::now();
    tempCloud.header.frame_id = "map";
    if (pc_map_pub_.getNumSubscribers() != 0)
        pc_map_pub_.publish(tempCloud);
}

void ShowPub::pubFull(pcl::PointCloud<PointType>::Ptr p_map){
    sensor_msgs::PointCloud2 tempCloud;
    pcl::toROSMsg(*p_map, tempCloud);
    tempCloud.header.stamp = ros::Time::now();
    tempCloud.header.frame_id = "map";
    if (pc_full_pub_.getNumSubscribers() != 0)
        pc_full_pub_.publish(tempCloud);
}

void ShowPub::pubOdom(double* var){
    //pub odom
    nav_msgs::Odometry tracker_odom_cur_msg;
    tracker_odom_cur_msg.header.frame_id = "map";
    tracker_odom_cur_msg.child_frame_id = "lidar";
    tracker_odom_cur_msg.header.stamp = ros::Time::now();
    tracker_odom_cur_msg.pose.pose.position.x = var[4];
    tracker_odom_cur_msg.pose.pose.position.y = var[5];
    tracker_odom_cur_msg.pose.pose.position.z = var[6];
    tracker_odom_cur_msg.pose.pose.orientation.x = var[0];
    tracker_odom_cur_msg.pose.pose.orientation.y = var[1];
    tracker_odom_cur_msg.pose.pose.orientation.z = var[2];
    tracker_odom_cur_msg.pose.pose.orientation.w = var[3];
    odom_pub_.publish(tracker_odom_cur_msg);
}

void ShowPub::pubImuIntegOdom(nav_msgs::Odometry odom){
    if(odom.header.frame_id =="")
        return;
    imu_integ_odom_pub_.publish(odom);
}


}
